#include <vector>
#include <algorithm>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMavFrame.h>

#include <geometry_msgs/TwistStamped.h>
 
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/Bool.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Imu.h> 
#include <std_msgs/Float32MultiArray.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// #include <sensor_msgs/LaserScan.h>  qaq
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Altitude.h>
#include <mavros_msgs/ActuatorControl.h>
#include <mavros_msgs/WaypointList.h>

// #include <pcl/point_cloud.h>
// #include <pcl/point_types.h>
// #include <pcl_conversions/pcl_conversions.h>
// #include <pcl/io/io.h>
// #include <pcl/io/pcd_io.h>
// #include <pcl/segmentation/sac_segmentation.h>
// #include <pcl/segmentation/extract_clusters.h>
// #include <pcl/kdtree/kdtree.h>
// #include <pcl/common/common.h>
// #include <pcl/filters/voxel_grid.h>

#include "quadrotor_msgs/PositionCommand.h"
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <unionsys_core.hpp>

#include <swarm_msgs/BoundingBoxes.h>
#include <swarm_msgs/BoundingBox.h>

RegisterMSN *unionsys_core = new RegisterMSN();

enum FSM_STATE { INIT, TAKEOFF, CRUISE, MISSION, PLANNER_SINGLE, PLANNER_MULTI, CHUZHI_HUMAN, CHUZHI_CAR, CHUZHI_OIL, CHUZHI_WEAPON, GO_EXPLORATION, EXPLORATION, RETURN, STOP};
std::string state_str[14] = { "INIT",
                        "TAKEOFF",
                        "CRUISE",
                        "MISSION",
                        "PLANNER_SINGLE", 
                        "PLANNER_MULTI",
                        "CHUZHI_HUMAN",
                        "CHUZHI_CAR",
                        "CHUZHI_OIL",
                        "CHUZHI_WEAPON",
                        "GO_EXPLORATION",
                        "EXPLORATION",
                        "RETURN",
                        "STOP"};
FSM_STATE exec_state;

ros::Time last_offboard_request;
ros::Time last_pub_goal_time;
ros::Time last_pub_tracking_time;
ros::Time total_track_time;
ros::Time last_path_send;




mavros_msgs::CommandBool arm_cmd;
mavros_msgs::SetMode offb_set_mode;
mavros_msgs::SetMavFrame setmavframe;

//控制pid
float P_pos = 1.0;
float I_pos = 0.0;
float D_pos = 0.2;

const float P_yaw = 1.00;
const float I_yaw = 0.00;
const float D_yaw = 0.00;
const float P_z = 1.00;

double last_calc_time = 0;

const float YAW_I_LIMIT = 2.0;
const float YAW_RATE_LIMIT = 15.0;

#define POS_I_DEAD_ZONE 0.5 
#define YAW_DEAD_ZONE 0
#define HEIGHT_DEAD_ZONE 0.05
#define XY_DEAD_ZONE 0.05
//rc channels
int channel1_value;
int channel2_value;
int channel3_value;
int channel4_value;
int channel5_value;
int channel6_value;
int channel7_value;
int channel8_value;

const float deg2rad = 3.1415926535798/180.0;
const float rad2deg = 180.0/3.1415926535798;

//launch file variables
double mission_number;
bool use_sitl;
double max_vel_beishu;



double planner_target_point_x;
double planner_target_point_y;
double planner_target_point_z;




double exploration_start_point_x;
double exploration_start_point_y;
double exploration_start_point_z;
double exploration_start_yaw;

double exploration_min_x;
double exploration_min_y;
double exploration_max_x;
double exploration_max_y;

double endpoint_point_x;
double endpoint_point_y;
double endpoint_point_z;


double exploration_z = 1.0;

int vel_command_cnt = 0;
int print_state_cnt = 0;
int waypoint_exec_num = 0;
int pipline_waypoint_num = 0;

int planner_cnt = 0;

bool exploration_finished_flag = false;
bool stop_to_bizhang_flag = false;
bool circle_update_flag = false;
bool pipeline_update_flag = false;

double last_force_yaw_error=0.0;
Eigen::Vector3d last_force_with_forward(0,0,0);
double last_time_force = 0.0;
//ros node define
mavros_msgs::State current_state;

ros::Subscriber local_odom_sub, lio_odom_sub;
ros::Subscriber rcin_sub;
ros::Subscriber state_sub;
ros::Subscriber planWaypointsub;
ros::Subscriber velocitysub;
ros::Subscriber altitude_sub, exploration_sub, pipline_force_sub, stair_force_sub, stoptobizhang_sub,return_flag_sub, startmission_sub; 
ros::Subscriber human_pose_sub,car_pose_sub,oil_pose_sub,weapon_pose_sub;

ros::Publisher  local_vel_pub, simple_goal_pub_, exploration_msg_pub, circle_finished_pub, control_plot, virtual_ceil_ground_pub;
ros::Publisher  new_plan_pub, max_vel_pub, waypoints_pub, chuzhi_msg_pub, fsm_state_pub;

ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;
ros::ServiceClient set_vechile_ned;

ros::Timer px4_control_timer;

geometry_msgs::Point current_point,current_point_fcu, body_vel;
geometry_msgs::Point planned_point;
geometry_msgs::Point cruise_point;
geometry_msgs::Point planned_velocity;
geometry_msgs::Point current_vel;
geometry_msgs::Quaternion current_angle, current_angle_fcu;
geometry_msgs::Vector3 curr_plane_angle, curr_plane_angle_fcu;

geometry_msgs::Point target_point_human;
geometry_msgs::Point target_point_car;
geometry_msgs::Point target_point_oil;
geometry_msgs::Point target_point_weapon;
double nearest_oil_offset = 5;
geometry_msgs::Point nearest_lio_offset_byoil;

std::vector<geometry_msgs::Point> human_chuzhi_list;
std::vector<geometry_msgs::Point> car_chuzhi_list;
std::vector<geometry_msgs::Point> oil_chuzhi_list;
std::vector<geometry_msgs::Point> weapon_chuzhi_list;

mavros_msgs::WaypointList waypoints_pipline;
mavros_msgs::WaypointList waypoints_stair;
mavros_msgs::WaypointList waypoints_return;


geometry_msgs::Point error_pos_last, error_pos_integrated, attitude_expect, velocity_expected;
geometry_msgs::Point stair_heli_force, pipline_heli_force;

float error_yaw_last = 0.0, error_yaw_integrated = 0.0;


bool planned_update_flag = false;
bool pub_planner_goal_once = true;
bool pub_planner_waypoints_once = true;
bool stair_pose_update_flag= false;

float planned_yaw_angle;
float planned_yaw_dot;
double target_yaw_walker;

Eigen::Quaterniond current_angle_Q;

template <typename T>
T limit(T a, T max)
{
  if(a > max)
    a = max;
  if(a < -max)
    a = -max;
  return a;
}

struct Point3D {
    float x, y, z;
 
    // 构造函数，方便初始化
    Point3D(float x = 0, float y = 0, float z = 0) : x(x), y(y), z(z) {}
};

bool human_chuzhi_start_flag = false;
bool car_chuzhi_start_flag = false;
bool oil_chuzhi_start_flag = false;
bool weapon_chuzhi_start_flag = false;

// bool human_chuzhi_finish_flag = false;
bool car_chuzhi_finish_flag = false;
// bool oil_chuzhi_finish_flag = false;
// bool weapon_chuzhi_finish_flag = false;

int human_chuzhi_finish_flag = 0;
int oil_chuzhi_finish_flag = 0;
int weapon_chuzhi_finish_flag = 0;


double human_update_time = 0.0;
double car_update_time = 0.0;
double oil_update_time = 0.0;
double weapon_update_time = 0.0;

double human_target_distance = 0.0;
double car_target_distance = 0.0;
double oil_target_distance = 0.0;
double weapon_target_distance = 0.0;

double human_chuzhi_start_time = 0.0;
double car_chuzhi_start_time = 0.0;
double oil_chuzhi_start_time = 0.0;
double weapon_chuzhi_start_time = 0.0;

double human_chuzhi_time = 0.0;
double car_chuzhi_time = 0.0;
double oil_chuzhi_time = 0.0;
double weapon_chuzhi_time = 0.0;

double vehicle_static_time = 0.0;
double vehicle_go_time = 0.0;
bool vehicle_static_flag = false;

bool return_flag = false;


std::vector<Point3D> vehicle_offset = {

    {0.0, 0.0, 0.0},
    {0.0, 5.8, 0.0},
    {-2.5, 0.0, -0.5}, 
    {-2.5, 5.8, 0.5},

    {-5.0, 0.0, 0.0},
    {-5.0, 6.0, 0.0},

    {-4.0, 0.0, 0.0},
    {-4.0, 2.0, 0.0},
    {-4.0, 4.0, 0.0},

    {4.0, 2.0, 0.0},
    {2.0, 2.0, 0.0},
    {0.0, 2.0, 0.0},
    //10架室外侦察巡逻
    {18.0, 4.0, 0.0},
    {16.0, 4.0, 0.0},
    {14.0, 4.0, 0.0},
    {12.0, 4.0, 0.0},
    {10.0, 4.0, 0.0},
    {8.0, 4.0, 0.0},
    {6.0, 4.0, 0.0},
    {4.0, 4.0, 0.0},
    {2.0, 4.0, 0.0},
    {0.0, 4.0, 0.0}
};

Point3D self_offset;
Point3D target_goal_from_gc;

std::vector<Point3D> waypoints_1 = { //106
    //go to mission start
    {0, -0.7, 1.2},
    {3.5, -0.7, 1.2},
    {7, -0.7, 1.2},
    {20.5, 1, 1.5},
    {20.5, 1, 6.75},
    {20.5, 1, 12.35},
    {20.5, -10.5, 12.35},
    {15, -10.5, 12.35},
    {20.5, 4.2, 12.35},
    {26, 4.2, 12.35},
    // {11.3, 0.4, 12.35},
    // {11.3, 0.4, 10.5},
    // {11.3, -10.5, 10.5},
    // {27, -10.5, 10.5},
    {26, 4.2, 10.5},
    {30, 4.2, 10.5},
    {28, 4.2, 10.5},
    {20.5, 2.5, 12.35},
    {20.5, 2.5, 6.75},
    {20.5, 2.5, 1.5},
    {15, 2.5, 1.5},

};
std::vector<Point3D> waypoints_2 = { //大仓库
    {0, 7.15, 1.2},
    {3.5, 7.15, 1.2},
    {7, 7.15, 1.2},
    {20.5, 7.15, 1.5},
    {20.5, 7.15, 6.75},
    {20.5, 7.15, 12.35},
    {20.5, 16.8, 12.35},
    {15, 16.8, 12.35},
    {20.5, 10.3, 12.35},
    {26, 10.3, 12.35},
    {26, 10.3, 10.5},
    {30, 10.3, 10.5},
    {26, 10.3, 10.5},
    // {26, 7.15, 12.35},
    {20.5, 7.15, 12.35},
    {20.5, 7.15, 6.75},
    {20.5, 7.15, 1.5},
};


std::vector<Point3D> waypoints_3 = {  //102
    {0, -0.7, 1.2},
    {3.5, -0.7, 1.2},
    {7, -0.7, 1.2},
    {20.5, 1, 1.5},
    {20.5, 1, 6.75},
    
    {20.5, -10.5, 7.5},
    {15, -10.5, 7.5},
    {20.5, 4.2, 7.5},
    {26, 4.2, 7.5},
    // {11.3, 0.4, 12.35},
    // {11.3, 0.4, 10.5},
    // {11.3, -10.5, 10.5},
    // {27, -10.5, 10.5},
    {26, 4.6, 6},
    {30, 4.6, 6},
    {28, 4.6, 6},
    {20.5, 2.5, 7.5},
    {20.5, 2.5, 1.5},
    {15, 2.5, 1.5},

};
 
std::vector<Point3D> waypoints_4 = { //101
    {0, 7.15, 1.2},
    {3.5, 7.15, 1.2},
    {7, 7.15, 1.2},
    {20.5, 7.15, 1.5},
    {20.5, 7.15, 7.5},
    // {20.5, 7.15, 12.35},
    {20.5, 16.8,  7.5},
    {15, 16.8, 7.5},
    {20.5, 10.8, 7.5},
    {26, 10.8, 7.5},
    {26, 10.8, 5.5},
    {30, 10.8, 5.5},
    {26, 10.8, 5.5},
    // {26, 7.15, 12.35},
    {20.5, 7.15, 7.5},
    {20.5, 7.15, 1.5},
};


std::vector<Point3D> waypoints_5 = { //大仓库
    {0.0, -0.7, 1.2},
    {3.5, -0.7, 1.2},
    {7, -0.7, 1.2},
    {11.3, -10.5, 1.5},
    {20.5, -10.5, 1.5},
    {20.5, 1.0, 1.5},
    {14.0, 1.0, 1.5},

};
std::vector<Point3D> waypoints_6 = { //大仓库
    {0, 7.15, 1.2},
    {3.5, 7.15, 1.2},
    {7, 7.15, 1.2},
    {11.3, 16.8, 1.5},
    {20.5, 16.8, 1.5},
    {20.5, 7.15, 1.5},
    {14.0, 7.15, 1.5},
};

std::vector<Point3D> waypoints_7 = { //一楼深处
    //go to mission start
    {10.0, -2.5, 1.5},
    {5.0, -2.5, 1.5},
    {5.0, -11.0, 1.5},
    
    {9.0, -33.0, 1.5},
    
    // // 一楼茶水间
    // {15.3, -33.0, 1.5},
    // {15.3, -28.1, 1.5},
    // {15.3, -33.0, 1.5},
    // 一楼假人房间
    {18.6, -33.1, 1.5},
    {18.6, -28.3, 1.5},
    {18.6, -33.1, 1.5},
    
    {10.0, -33.0, 1.5},
    //back home
    {10.0, -33.0, 1.5},
    {10.0, -33.0, 1.5},
    {10.0, -33.0, 1.5}
};



std::vector<Point3D> waypoints_8 = { //二楼深处
    //go to mission start
    {8.0, -2.5, 1.5},
    {5.0, -2.5, 1.5},
    {5.0, -11.0, 1.5},
    // mission 1
    {10.5, -26.5, 4.8}, 
    {15.6, -26.5, 4.8},
    {10.5, -26.5, 4.8}, 


    // mission 2
    {10.5, -33.5, 4.8},
    {16.5, -33.5, 4.8},
    {10.5, -33.5, 4.8},


    //back home
    {10.0, -25.4, 4.8},
    {10.0, -25.4, 1.5},
    {10.0, -25.4, 1.5},
};

std::vector<Point3D> waypoints_9 = { //一楼近处
    //go to mission start
    {6.0, -2.5, 1.5},
    {5.0, -2.5, 1.5},
    {5.0, -11.0, 1.5},
    
    // 一楼油桶房间
    {11.3, -20.4, 1.5},
    {15.6, -20.4, 1.5}, 
    {11.3, -20.4, 1.5},

    //back home
    {10.0, -20.0, 1.5},
    {10.0, -20.0, 1.5},
    {10.0, -20.0, 1.5}
};

std::vector<Point3D> waypoints_10 = { //二楼近处
    //go to mission start
    {4.0, -2.5, 1.5},
    {5.0, -2.5, 1.5},
    {5.0, -11.0, 1.5},

    {10.5, -10.0, 4.8}, 
    {15.5, -9.0, 4.8}, 
    {10.5, -10.0, 4.8}, 
    // mission 1
    {10.0, -12.5, 4.8}, 
    {16.0, -12.5, 4.8},
    {10.0, -12.5, 4.8}, 

    //back home
    {10.0, -15.0, 4.8},
    {5.0, -11.0, 1.5},
    {5.0, -0.0, 1.5},
};

std::vector<Point3D> waypoints_11 = { //大厅靠近房间
    //go to mission start
    {2.0, -2.5, 1.5},
    {5.0, -2.5, 1.5},
    {5.0, -11.0, 1.5},
    
    // 一楼油桶房间
    {3.0, -15.5, 1.5},
    {3.0, -30.0, 1.5}, 
    {3.0, -15.5, 1.5},

    {3.0, -15.5, 1.5},
    {3.0, -15.5, 1.5},
    {3.0, -15.5, 1.5}
};

std::vector<Point3D> waypoints_12 = { //大厅远离房间
    //go to mission start
    {0.0, -2.5, 1.5},
    {5.0, -2.5, 1.5},
    {5.0, -11.0, 1.5},
    
    // 一楼油桶房间
    {-1.0, -15.5, 1.5},
    {-1.0, -30.0, 1.5}, 
    {-1.0, -15.5, 1.5},

    {-1.0, -15.5, 1.5},
    {-1.0, -15.5, 1.5},
    {-1.0, -15.5, 1.5}
};

std::vector<Point3D> waypoints_13 = { //indoortest
    //go to mission start
    {18.0, 8.0, 1.5},
    {20.0, 8.0, 1.5},
    {22.9, 9.0, 4.8},
        // mission 1
    {12.0, 10.0, 4.8}, 
    {12.0, 17.0, 4.8},
    {12.0, 10.0, 4.8}, 

    // mission 2
    {25.6, 10.0, 4.8},
    {25.6, 15.6, 4.8},
    {25.6, 10.0, 4.8},

    // mission 2
    {33.6, 10.0, 4.8},
    {33.6, 18.5, 4.8},
    {33.6, 10.0, 4.8},


    //back home
    {20.6, 10.0, 4.8},
    {18.0, 8.0, 1.5},
    {15.0, 8.0, 1.5}
};

std::vector<Point3D> waypoints_14 = { //indoortest
    //go to mission start
    {18.0, 8.0, 1.5},
    {20.0, 8.0, 1.5},
    {22.9, 9.0, 4.8},
        // mission 1
    {12.0, 10.0, 4.8}, 
    {12.0, 17.0, 4.8},
    {12.0, 10.0, 4.8}, 

    // mission 2
    {25.6, 10.0, 4.8},
    {25.6, 15.6, 4.8},
    {25.6, 10.0, 4.8},

    // mission 2
    {33.6, 10.0, 4.8},
    {33.6, 18.5, 4.8},
    {33.6, 10.0, 4.8},


    //back home
    {20.6, 10.0, 4.8},
    {18.0, 8.0, 1.5},
    {15.0, 8.0, 1.5}
};

std::vector<Point3D> waypoints_15 = { //indoortest
    //go to mission start
    {18.0, 8.0, 1.5},
    {20.0, 8.0, 1.5},
    {22.9, 9.0, 4.8},
        // mission 1
    {12.0, 10.0, 4.8}, 
    {12.0, 17.0, 4.8},
    {12.0, 10.0, 4.8}, 

    // mission 2
    {25.6, 10.0, 4.8},
    {25.6, 15.6, 4.8},
    {25.6, 10.0, 4.8},

    // mission 2
    {33.6, 10.0, 4.8},
    {33.6, 18.5, 4.8},
    {33.6, 10.0, 4.8},


    //back home
    {20.6, 10.0, 4.8},
    {18.0, 8.0, 1.5},
    {15.0, 8.0, 1.5}
};

std::vector<Point3D> waypoints_16 = { //indoortest
    //go to mission start
    {18.0, 8.0, 1.5},
    {20.0, 8.0, 1.5},
    {22.9, 9.0, 4.8},
        // mission 1
    {12.0, 10.0, 4.8}, 
    {12.0, 17.0, 4.8},
    {12.0, 10.0, 4.8}, 

    // mission 2
    {25.6, 10.0, 4.8},
    {25.6, 15.6, 4.8},
    {25.6, 10.0, 4.8},

    // mission 2
    {33.6, 10.0, 4.8},
    {33.6, 18.5, 4.8},
    {33.6, 10.0, 4.8},


    //back home
    {20.6, 10.0, 4.8},
    {18.0, 8.0, 1.5},
    {15.0, 8.0, 1.5}
};

std::vector<Point3D> waypoints_17 = { //indoortest
    //go to mission start
    {18.0, 8.0, 1.5},
    {20.0, 8.0, 1.5},
    {22.9, 9.0, 4.8},
        // mission 1
    {12.0, 10.0, 4.8}, 
    {12.0, 17.0, 4.8},
    {12.0, 10.0, 4.8}, 

    // mission 2
    {25.6, 10.0, 4.8},
    {25.6, 15.6, 4.8},
    {25.6, 10.0, 4.8},

    // mission 2
    {33.6, 10.0, 4.8},
    {33.6, 18.5, 4.8},
    {33.6, 10.0, 4.8},


    //back home
    {20.6, 10.0, 4.8},
    {18.0, 8.0, 1.5},
    {15.0, 8.0, 1.5}
};

std::vector<Point3D> waypoints_18 = { //indoortest
    //go to mission start
    {18.0, 8.0, 1.5},
    {20.0, 8.0, 1.5},
    {22.9, 9.0, 4.8},
        // mission 1
    {12.0, 10.0, 4.8}, 
    {12.0, 17.0, 4.8},
    {12.0, 10.0, 4.8}, 

    // mission 2
    {25.6, 10.0, 4.8},
    {25.6, 15.6, 4.8},
    {25.6, 10.0, 4.8},

    // mission 2
    {33.6, 10.0, 4.8},
    {33.6, 18.5, 4.8},
    {33.6, 10.0, 4.8},


    //back home
    {20.6, 10.0, 4.8},
    {18.0, 8.0, 1.5},
    {15.0, 8.0, 1.5}
};

std::vector<Point3D> waypoints_19 = { //indoortest
    //go to mission start
    {18.0, 8.0, 1.5},
    {20.0, 8.0, 1.5},
    {22.9, 9.0, 4.8},
        // mission 1
    {12.0, 10.0, 4.8}, 
    {12.0, 17.0, 4.8},
    {12.0, 10.0, 4.8}, 

    // mission 2
    {25.6, 10.0, 4.8},
    {25.6, 15.6, 4.8},
    {25.6, 10.0, 4.8},

    // mission 2
    {33.6, 10.0, 4.8},
    {33.6, 18.5, 4.8},
    {33.6, 10.0, 4.8},


    //back home
    {20.6, 10.0, 4.8},
    {18.0, 8.0, 1.5},
    {15.0, 8.0, 1.5}
};

std::vector<Point3D> waypoints_20 = { //indoortest
    //go to mission start
    {18.0, 8.0, 1.5},
    {20.0, 8.0, 1.5},
    {22.9, 9.0, 4.8},
        // mission 1
    {12.0, 10.0, 4.8}, 
    {12.0, 17.0, 4.8},
    {12.0, 10.0, 4.8}, 

    // mission 2
    {25.6, 10.0, 4.8},
    {25.6, 15.6, 4.8},
    {25.6, 10.0, 4.8},

    // mission 2
    {33.6, 10.0, 4.8},
    {33.6, 18.5, 4.8},
    {33.6, 10.0, 4.8},


    //back home
    {20.6, 10.0, 4.8},
    {18.0, 8.0, 1.5},
    {15.0, 8.0, 1.5}
};

std::vector<Point3D> waypoints_21 = { //indoortest
    //go to mission start
    {18.0, 8.0, 1.5},
    {20.0, 8.0, 1.5},
    {22.9, 9.0, 4.8},
        // mission 1
    {12.0, 10.0, 4.8}, 
    {12.0, 17.0, 4.8},
    {12.0, 10.0, 4.8}, 

    // mission 2
    {25.6, 10.0, 4.8},
    {25.6, 15.6, 4.8},
    {25.6, 10.0, 4.8},

    // mission 2
    {33.6, 10.0, 4.8},
    {33.6, 18.5, 4.8},
    {33.6, 10.0, 4.8},


    //back home
    {20.6, 10.0, 4.8},
    {18.0, 8.0, 1.5},
    {15.0, 8.0, 1.5}
};

std::vector<Point3D> waypoints_22 = { //indoortest
    //go to mission start
    {18.0, 8.0, 1.5},
    {20.0, 8.0, 1.5},
    {22.9, 9.0, 4.8},
        // mission 1
    {12.0, 10.0, 4.8}, 
    {12.0, 17.0, 4.8},
    {12.0, 10.0, 4.8}, 

    // mission 2
    {25.6, 10.0, 4.8},
    {25.6, 15.6, 4.8},
    {25.6, 10.0, 4.8},

    // mission 2
    {33.6, 10.0, 4.8},
    {33.6, 18.5, 4.8},
    {33.6, 10.0, 4.8},


    //back home
    {20.6, 10.0, 4.8},
    {18.0, 8.0, 1.5},
    {15.0, 8.0, 1.5}
};



std::vector<Point3D> waypoints_self;
mavros_msgs::WaypointList waypoints_planner_multi;

bool local_odom_flag = false;
bool start_mission_flag = false;
bool get_mission_flag = false;
bool target_goal_gc_flag = false;
double current_point_z = 0.0;

void position_pid_control(geometry_msgs::Point current_set_point,geometry_msgs::Point current_local_point,float velocity_limit,float target_yaw, float self_yaw, bool ifplan);
void vel_command_send(geometry_msgs::Point vel, double yaw_rate);
void changeFSMExecState(FSM_STATE new_state, std::string pos_call);
void printFSMExecState();


void set_cruise_state();
void waypoints_no_planner(mavros_msgs::WaypointList waypoints_plan, double yaw, double yuzhi);
void waypoints_planner(mavros_msgs::WaypointList waypoints_pipline, double yaw, bool specified_yaw);
void simple_goal_planner(double target_x, double target_y, double target_z, double yaw, bool specified_yaw);
void continuously_goal_planner(double target_x, double target_y, double target_z);